﻿/*
 * ProjMath
 * 
 * initial implementation : 10.1.2009
 *
 * Copyright (c) Adam Sida, asida@centrum.cz.
 * All rights reserved.
 * 
 * for license see Petra.Gis.License.txt
 */
using System;
using System.Collections.Generic;


namespace Petra.Gis.ReferenceSystemServices.Transformations
{
  public static class ProjMath
  {

    /// <summary>
    /// R2D
    /// </summary>
    public const double R2D = 180 / Math.PI;


    /// <summary>
    /// D2R
    /// </summary>
    public const double D2R = Math.PI / 180;


    //// defines some usefull constants that are used in the projection routines
    ///// <summary>
    ///// PI
    ///// </summary>
    //public const double PI = Math.PI;


    public static readonly int MAX_ITERATIONS = 15;


    /// <summary>
    /// Half of PI
    /// </summary>
    public static readonly double HALF_PI = (Math.PI * 0.5);


    /// <summary>
    /// PI/4
    /// </summary>
    public static readonly double S45 = Math.PI / 4.0;


    /// <summary>
    /// PI * 2
    /// </summary>
    public static readonly double TWO_PI = (Math.PI * 2.0);


    /// <summary>
    /// EPSLN = 1.0e-10, mean very small value
    /// </summary>
    public static readonly double EPSLN = 1.0e-10;


    /// <summary>
    /// EPSLN = 1.0e-6, mean very small value
    /// </summary>
    public static readonly double EPSLN6 = 1.0e-6;


    /// <summary>
    /// S2R
    /// </summary>
    public static readonly double S2R = 4.848136811095359e-6;


    /// <summary>
    /// MAX_VAL
    /// </summary>
    public static readonly double MAX_VAL = 4;


    /// <summary>
    /// prjMAXLONG
    /// </summary>
    public static readonly double prjMAXLONG = 2147483647;


    /// <summary>
    /// DBLLONG
    /// </summary>
    public static readonly double DBLLONG = 4.61168601e18;





    /// <summary>
    /// Returns the cube of a number.
    /// </summary>
    /// <param name="x"> </param>
    public static double Cube(double x)
    {
      return Math.Pow(x, 3);   /* x^3 */
    }





    /// <summary>
    /// Returns the quad of a number.
    /// </summary>
    /// <param name="x"> </param>
    public static double Quad(double x)
    {
      return Math.Pow(x, 4);  /* x^4 */
    }




    ///// <summary>
    ///// 
    ///// </summary>
    ///// <param name="A"></param>
    ///// <param name="B"></param>
    ///// <returns></returns>
    //public static double Gmax(ref double A, ref double B)
    //{
    //  return Math.Max(A, B); /* assign maximum of a and b */
    //}






    ///// <summary>
    ///// 
    ///// </summary>
    ///// <param name="A"></param>
    ///// <param name="B"></param>
    ///// <returns></returns>
    //public static double Gmin(ref double A, ref double B)
    //{
    //  return ((A) < (B) ? (A) : (B)); /* assign minimum of a and b */
    //}






    /// <summary>
    /// Imod
    /// </summary>
    /// <param name="A"></param>
    /// <param name="B"></param>
    /// <returns></returns>
    public static double Imod(double A, double B)
    {
      return (A) - (((A) / (B)) * (B)); /* Integer mod function */
    }





    ///<summary>
    ///Function to return the Sign of an argument
    ///</summary>
    public static double Sign(double x)
    {
      if (x < 0.0)
        return (-1);
      else
        return (1);
    }





    /// <summary>
    ///  Ensures that the specified longitude stay within &plusmn;&pi; radians. This method
    /// is typically invoked after geographic coordinates are transformed. This method may add
    /// or substract some amount of 2&pi; radians to <var>x</var>.
    /// </summary>
    /// <param name="lon">longitude in radians</param>
    /// <returns>The longitude in the range &plusmn;&pi; radians.</returns>
    public static double RollLongitude(double lon)
    {
        return lon - (2 * Math.PI) * Math.Floor(lon / (2 * Math.PI) + 0.5);
    }





    /// <summary>
    /// update given lontitude to fit into range after equation
    /// </summary>
    /// <param name="x"></param>
    /// <returns></returns>
    public static double AdjustLongitude(double x)
    {
      long count = 0;
      for (; ; )
      {
        if (Math.Abs(x) <= Math.PI)
          break;
        else
          if (((long)Math.Abs(x / Math.PI)) < 2)
            x = x - (Sign(x) * TWO_PI);
          else
            if (((long)Math.Abs(x / TWO_PI)) < prjMAXLONG)
            {
              x = x - (((long)(x / TWO_PI)) * TWO_PI);
            }
            else
              if (((long)Math.Abs(x / (prjMAXLONG * TWO_PI))) < prjMAXLONG)
              {
                x = x - (((long)(x / (prjMAXLONG * TWO_PI))) * (TWO_PI * prjMAXLONG));
              }
              else
                if (((long)Math.Abs(x / (DBLLONG * TWO_PI))) < prjMAXLONG)
                {
                  x = x - (((long)(x / (DBLLONG * TWO_PI))) * (TWO_PI * DBLLONG));
                }
                else
                  x = x - (Sign(x) * TWO_PI);
        count++;
        if (count > MAX_VAL)
          break;
      }
      return (x);
    }





    /// <summary>
    /// Iteratively solve equation (7-9) from Snyder.
    /// </summary>
    /// <param name="ts">latitude</param>
    /// <param name="e">ellipsiod eccentricity</param>
    /// <returns>iterated latitude</returns>
    public static double Cphi2(double ts, double e)
    {

      double dphi, con;

      double eccnth = 0.5 * e;
      double phi = (Math.PI / 2) - 2.0 * Math.Atan(ts);

      for (int i = 0; i < MAX_ITERATIONS; i++)
      {
        con = e * Math.Sin(phi);
        dphi = (Math.PI / 2) - 2.0 * Math.Atan(ts * Math.Pow((1 - con) / (1 + con), eccnth)) - phi;
        phi += dphi;
        if (Math.Abs(dphi) <= EPSLN)
        {
          return phi;
        }
      }
      throw new ApplicationException("no convergency");

    }



    /// <summary>
    /// Function needed for the true scale latitude (Snyder 14-15)
    /// </summary>
    /// <param name="e2">squared ellipsoid eccentricity</param>
    /// <param name="sinphi">sine of true scale latitude</param>
    /// <param name="cosphi">cosine true scale latitude</param>
    /// <returns></returns>
    public static double Msfn(double e2, double sinphi, double cosphi)
    {
      return cosphi / Math.Sqrt(1.0 - (sinphi * sinphi) * e2);
    } 





    /// <summary>
    /// Computes the constant small m which is the radius of
    /// a parallel of latitude, phi, divided by the semimajor axis.
    /// </summary>
    /// <param name="e">ellipsoid eccentricity</param>
    /// <param name="sinphi">sine of true scale latitude</param>
    /// <param name="cosphi">cosine true scale latitude</param>
    /// <returns>small m constant</returns>
    public static double Msfnz(double e, double sinphi, double cosphi)
    {

      double con = e * sinphi;

      return ((cosphi / (Math.Sqrt(1.0 - con * con))));
    }





    /// <summary>
    /// Function to compute constant small q which is the radius of a 
    /// parallel of latitude, phi, divided by the semimajor axis. 
    /// </summary>
    public static double Qsfnz(double eccent, double sinphi, double cosphi)
    {
      double con;

      if (eccent > 1.0e-7)
      {
        con = eccent * sinphi;
        return ((1.0 - eccent * eccent) * (sinphi / (1.0 - con * con) - (.5 / eccent) *
          Math.Log((1.0 - con) / (1.0 + con))));
      }
      else
        return (2.0 * sinphi);
    }







    /// <summary>
    /// Function to calculate the sine and cosine in one call.  Some computer
    /// systems have implemented this function, resulting in a faster implementation
    /// than calling each function separately.  It is provided here for those
    /// computer systems which don`t implement this function
    /// </summary>
    public static void Sincos(double val, out double sin_val, out double cos_val)
    {
      sin_val = Math.Sin(val);
      cos_val = Math.Cos(val);
    }






    ///// <summary>
    ///// Function to compute the constant small t for use in the forward
    ///// computations in the Lambert Conformal Conic and the Polar
    ///// Stereographic projections.
    ///// </summary>
    //public static double Tsfnz(double eccent, double phi,  double sinphi )
    //{
    //  double con, com;

    //  con = eccent * sinphi;
    //  com = .5 * eccent;
    //  con = Math.Pow(((1.0 - con) / (1.0 + con)), com);
    //  return (Math.Tan(.5 * (HALF_PI - phi)) / con);
    //}





    /// <summary>
    /// Function to compute the constant small t for use in the forward
    /// computations in the Lambert Conformal Conic and the Polar
    /// Stereographic projections.
    /// Compute function (15-9) and (9-13) from Snyder.
    /// </summary>
    /// <param name="e">ellipsoid eccentricity</param>
    /// <param name="phi">latitude</param>
    /// <param name="sinphi">sine of latitude</param>
    /// <returns>constant small t</returns>
    public static double Tsfnz(double e, double phi, double sinphi)
    {
      sinphi *= e;
      
      //NOTE: change sign to get the equivalent of Snyder (7-7).
      return Math.Tan(0.5 * (HALF_PI - phi)) / Math.Pow((1 - sinphi) / (1 + sinphi), 0.5 * e);
    }





    /// <summary>
    /// 
    /// 
    /// </summary>
    /// <param name="eccent"></param>
    /// <param name="qs"></param>
    /// <param name="flag"></param>
    /// <returns></returns>
    public static double Phi1z(double eccent, double qs, out long flag)
    {
      double eccnts;
      double dphi;
      double con;
      double com;
      double sinpi;
      double cospi;
      double phi;
      flag = 0;
      //double Asinz();
      long i;

      phi = Asinz(.5 * qs);
      if (eccent < EPSLN)
        return (phi);
      eccnts = eccent * eccent;
      for (i = 1; i <= 25; i++)
      {
        Sincos(phi, out sinpi, out cospi);
        con = eccent * sinpi;
        com = 1.0 - con * con;
        dphi = .5 * com * com / cospi * (qs / (1.0 - eccnts) - sinpi / com +
          .5 / eccent * Math.Log((1.0 - con) / (1.0 + con)));
        phi = phi + dphi;
        if (Math.Abs(dphi) <= 1e-7)
          return (phi);
      }
      //p_error ("Convergence error","Phi1z-conv");
      //ASSERT(FALSE);
      throw new ApplicationException("Convergence error.");
    }






    ///<summary>
    ///Function to eliminate roundoff errors in asin
    ///</summary>
    public static double Asinz(double con)
    {
      if (Math.Abs(con) > 1.0)
      {
        if (con > 1.0)
          con = 1.0;
        else
          con = -1.0;
      }
      return (Math.Asin(con));
    }





    /// <summary>
    /// Function to compute the latitude angle, phi2, for the inverse of the
    /// Lambert Conformal Conic and Polar Stereographic projections.
    /// </summary>
    /// <param name="eccentricity">Spheroid eccentricity</param>
    /// <param name="ts">Constant value t</param>
    /// <param name="flag">Error flag number</param>
    /// <returns></returns>
    public static double Phi2z(double eccentricity, double ts, out long flag)
    {
      double con;
      double dphi;
      double sinpi;
      long i;

      flag = 0;
      double eccnth = .5 * eccentricity;
      double chi = HALF_PI - 2 * Math.Atan(ts);
      for (i = 0; i <= 15; i++)
      {
        sinpi = Math.Sin(chi);
        con = eccentricity * sinpi;
        dphi = HALF_PI - 2 * Math.Atan(ts * (Math.Pow(((1.0 - con) / (1.0 + con)), eccnth))) - chi;
        chi += dphi;
        if (Math.Abs(dphi) <= .0000000001)
          return (chi);
      }
      throw new ApplicationException("Convergence error - Phi2z-conv");
    }






    ///<summary>
    ///Functions to compute the constants e0, e1, e2, and e3 which are used
    ///in a series for calculating the distance along a meridian.  The
    ///input x represents the eccentricity squared.
    ///</summary>
    public static double E0fn(double x)
    {
      return (1.0 - 0.25 * x * (1.0 + x / 16.0 * (3.0 + 1.25 * x)));
    }






    /// <summary>
    /// 
    /// </summary>
    /// <param name="x"></param>
    /// <returns></returns>
    public static double E1fn(double x)
    {
      return (0.375 * x * (1.0 + 0.25 * x * (1.0 + 0.46875 * x)));
    }






    /// <summary>
    /// 
    /// </summary>
    /// <param name="x"></param>
    /// <returns></returns>
    public static double E2fn(double x)
    {
      return (0.05859375 * x * x * (1.0 + 0.75 * x));
    }






    /// <summary>
    /// 
    /// </summary>
    /// <param name="x"></param>
    /// <returns></returns>
    public static double E3fn(double x)
    {
      return (x * x * x * (35.0 / 3072.0));
    }






    ///// <summary>
    ///// Function to compute the constant e4 from the input of the eccentricity
    ///// of the spheroid, x.  This constant is used in the Polar Stereographic
    ///// projection.
    ///// </summary>
    //public static double E4fn(double x)
    //{
    //  double con;
    //  double com;
    //  con = 1.0 + x;
    //  com = 1.0 - x;
    //  return (Math.Sqrt((Math.Pow(con, con)) * (Math.Pow(com, com))));
    //}






    /// <summary>
    /// Function computes the value of M which is the distance along a meridian
    /// from the Equator to latitude phi.
    /// </summary>
    public static double Mlfn(double e0, double e1, double e2, double e3, double phi)
    {
      return (e0 * phi - e1 * Math.Sin(2.0 * phi) + e2 * Math.Sin(4.0 * phi) - e3 * Math.Sin(6.0 * phi));
    }






    /// <summary>
    /// Function to calculate UTM zone number 
    /// </summary>
    /// <param name="lon">Longitude entered in Degrees</param>
    /// <returns>zone number</returns>
    public static long GetUtmZone(double lon)
    {
      return ((long)(((lon + 180.0) / 6.0) + 1.0));
    }






    /// <summary>
    /// Converts a longitude value in degrees to radians.
    /// </summary>
    /// <param name="x">The value in degrees to convert to radians.</param>
    /// <param name="edge">If true, -180 and +180 are valid, otherwise they are considered out of range.</param>
    /// <returns></returns>
    public static double LongitudeToRadians(double x, bool edge)
    {
      if (edge ? (x >= -180 && x <= 180) : (x > -180 && x < 180))
      {
        return Degrees2Radians(x);
      }
      throw new ArgumentOutOfRangeException("x", x, " not a valid longitude in degrees.");
    }






    /// <summary>
    /// Converts a latitude value in degrees to radians.
    /// </summary>
    /// <param name="y">The value in degrees to to radians.</param>
    /// <param name="edge">If true, -90 and +90 are valid, otherwise they are considered out of range.</param>
    /// <returns></returns>
    public static double LatitudeToRadians(double y, bool edge)
    {
      if (edge ? (y >= -90 && y <= 90) : (y > -90 && y < 90))
      {
        return Degrees2Radians(y);
      }
      throw new ArgumentOutOfRangeException("x", y, " not a valid latitude in degrees.");
    }




    /// <summary>
    /// converts input value from radians to decima degrees
    /// </summary>
    /// <param name="rad">angle in radians</param>
    /// <returns>value in decima degrees</returns>
    public static double Radians2Degrees(double rad)
    {
      return (R2D * rad);
    }





    /// <summary>
    /// Converts degrees to radians, multiply degrees by pi/180. 
    /// </summary>
    /// <param name="deg">angle in decimal degrees</param>
    /// <returns>angle in radians</returns>
    public static double Degrees2Radians(double deg)
    {
      return (D2R * deg);
    }




    /// <summary>
    /// Returns sqrt(x*x + y*y), the hypotenuse of a right-angled triangle
    /// with sides <i>x</i> and <i>y</i>.
    /// </summary>
    /// <param name="x">first param</param>
    /// <param name="y">second param</param>
    /// <returns>hypotenuse of a right-angled triangle</returns>
    public static double Hypot(double x, double y)
    {
      return Math.Sqrt(x * x + y * y);
    }





    /// <summary>
    /// Ensures that the latitude is within allowed limits (&plusmn;&pi;/2)
    /// </summary>
    /// <param name="lat">latitude in radians</param>
    /// <returns>true whe latitude is valid</returns>
    static bool IsValidLatitude(double lat)
	  {
      return lat >= -HALF_PI && lat <= HALF_PI;
    }

    



    /// <summary>
    /// Ensures that the longitue is within allowed limits (&plusmn;&pi;).
    /// </summary>
    /// <param name="lon">lontitude in radians</param>
    /// <returns>true whe lontitude is valid</returns>
    static bool IsValidLontitude(double lat)
	  {
      return lat >= -Math.PI && lat <= Math.PI;
    }




  }
}
